#ifndef FEATURIZER_H_
#define FEATURIZER_H_


#include "string"
#include <signal.h> 
#include <iostream>
#include "cstdio" 

// yarp
#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/os/Network.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/RateThread.h>
#include <yarp/os/Time.h>
#include <yarp/sig/Vector.h>
#include <yarp/math/Math.h>

#include <yarp/dev/Drivers.h>
#include <yarp/dev/CartesianControl.h>
#include <yarp/dev/PolyDriver.h>

YARP_DECLARE_DEVICES(icubmod)

using namespace yarp;
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace yarp::math;

using namespace std;
using namespace std;

#define CTRL_THREAD_PER     0.02    // [s]
#define PRINT_STATUS_PER    1.0     // [s]
#define MAX_TORSO_PITCH     30.0    // [deg]


class grasper : public yarp::os::RFModule
{
public:


    virtual bool 	configure(yarp::os::ResourceFinder &rf);
    virtual bool 	close();
    virtual bool 	interruptModule();
    virtual bool 	updateModule();
    virtual bool 	respond(const yarp::os::Bottle &command,yarp::os::Bottle &reply);
    virtual double 	getPeriod();
    void 		setArmString(string armName);

    grasper();


private:
	yarp::os::BufferedPort<yarp::os::Bottle> _portGraspComm;
	yarp::os::BufferedPort<yarp::os::Bottle> _portMoveArm;
	yarp::os::BufferedPort<yarp::os::Bottle> _portGetComm;
	
	PolyDriver        client;
	ICartesianControl *arm;
	string local,remote;
	Vector xd;
    	Vector od;


	int startup_context_id;
};

#endif 
